#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/gicp.h>

#include <geometry_utils/Transform3.h>
#include <sstream>
#include <fstream>
#include <iostream>
#include <string>
#include "time_counter.hpp"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <queue>
#include <thread>
#include <mutex>
#include <csignal>
#include <point2pixel_pin.h>
#include <point2pixel_fish.h>

using namespace std;

std::string itos(int i)
{
    std::stringstream s;
    s << i;
    return s.str();
}

template <typename Point>
void pubpc(ros::Publisher &pub, pcl::PointCloud<Point> &pc, std::string frame = "base_link")
{
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    pub.publish(msgs);
}

std::mutex mtx;
std::queue<sensor_msgs::Image> imgs;
std::queue<sensor_msgs::PointCloud2> clouds;

int i = 0;
const int jump = 5;

bool flag = true;

std::shared_ptr<Point2Pixel<pcl::PointXYZI>> p2p_ptr;
ros::Publisher pub;

void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    std::lock_guard<std::mutex> aa(mtx);
    clouds.push(*cloud);
    // cout << "点云时间" << std::setprecision(15) << cloud->header.stamp.toSec() << endl;
}

void ImageCallback(const sensor_msgs::Image::ConstPtr &img)
{
    std::lock_guard<std::mutex> aa(mtx);
    imgs.push(*img);
    // cout << "图像时间" << std::setprecision(15) << img->header.stamp.toSec() << endl;
}

void loop()
{
    int i = 0;
    const int jump = 5;
    std::string file_path = "/media/zwh/T7/ZWH/bags/dataset/nanchuan_det_seg/clouds/20230323111726/";
    while (flag)
    {
        sensor_msgs::Image img;
        sensor_msgs::PointCloud2 cloud;
        {
            //软同步
            std::unique_lock<std::mutex> aa(mtx);
            if (imgs.size() > 0 && clouds.size() > 0)
            {
                if (fabs(imgs.front().header.stamp.toSec() - clouds.front().header.stamp.toSec()) < 0.010)
                {
                    img = imgs.front();
                    imgs.pop();
                    cloud = clouds.front();
                    clouds.pop();
                    // got datas
                }
                else
                {
                    if (imgs.front().header.stamp.toSec() < clouds.front().header.stamp.toSec())
                    {
                        imgs.pop();
                    }
                    else
                    {
                        clouds.pop();
                    }
                    aa.unlock(); //释放互斥量
                    std::this_thread::sleep_for(std::chrono::microseconds(5000));
                    continue; //下一次while
                }
            }
            else
            {
                aa.unlock(); //释放互斥量
                std::this_thread::sleep_for(std::chrono::microseconds(50000));
                continue; //下一次while
            }
        }

        if ((i++) % jump != 0)
            continue;//隔几个处理一次

        pcl::PointCloud<pcl::PointXYZI> cloud4d;
        pcl::fromROSMsg(cloud, cloud4d);
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(boost::make_shared<sensor_msgs::Image>(img), "bgr8");
        }
        catch (cv_bridge::Exception &e)
        {
            return;
        }
        cv::Mat raw_image = cv_ptr->image;
        // cv::imwrite("/home/zwh/work_space/18xx/calibration-tool/TEST_DATA/hw2camera/mid_car_fish2lidar/" + std::to_string(i) + ".jpg", raw_image);
        cv::resize(raw_image, raw_image, cv::Size{1920, 1080});

        pcl::PointCloud<pcl::PointXYZRGBA> rgbacloud;
        for (const auto &pt : cloud4d)
        {
            pcl::PointXYZRGBA rgba_pt;
            cv::Point2d pt2d = p2p_ptr->loop(pt);
            if (pt2d.x > 0 && pt2d.x < 1920 && pt2d.y > 0 && pt2d.y < 1080)
            {
                rgba_pt.x = pt.x;
                rgba_pt.y = pt.y;
                rgba_pt.z = pt.z;
                rgba_pt.b = raw_image.at<cv::Vec3b>(pt2d.y, pt2d.x)[0];
                rgba_pt.g = raw_image.at<cv::Vec3b>(pt2d.y, pt2d.x)[1];
                rgba_pt.r = raw_image.at<cv::Vec3b>(pt2d.y, pt2d.x)[2];
                rgba_pt.a = pt.intensity;//我把强度存在a中
                rgbacloud.push_back(rgba_pt);
            }
        }
        cout << "数据处理" << std::setprecision(15) << img.header.stamp.toSec() << "     " << std::setprecision(15) << cloud.header.stamp.toSec()
        << "   output.size():"<< rgbacloud.size()<< endl;

        pcl::PointCloud<pcl::PointXYZRGB> rgbcloud;
        pcl::copyPointCloud(rgbacloud,rgbcloud);

        // 生成pcd
        pcl::PCDWriter writer;
        cout << "writing NO: " << i / jump << endl;
        std::string name = std::to_string(i / jump + 2023000000);
        writer.write(file_path + name + ".pcd", rgbcloud);

        sensor_msgs::PointCloud2 msgs;
        pcl::toROSMsg(rgbcloud, msgs);
        msgs.header.frame_id = "base_link";
        pub.publish(msgs);
    }
    cout << "正常退出本线程" << endl;
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "bag2pcd");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/pointcloud_lidar3", 10, velodyne_callback);
    ros::Subscriber sub_img = nh.subscribe<sensor_msgs::Image>("/camera79", 1, &ImageCallback);
    pub = nh.advertise<sensor_msgs::PointCloud2>("/rgba_cloud", 1);

    p2p_ptr.reset(new Point2PixelPin());

    std::string sensor_path = "/home/zwh/work_space/18xx/lidar_tool_ws/src/lidar_tool/p2p/nanchuan.yaml";
    cv::FileStorage fs(sensor_path, cv::FileStorage::READ);
    std::cout << "参数文件来自:" << sensor_path << std::endl;
    std::string camera_id = "P39";

    bool isfish;
    std::string pos;
    cv::Mat base2camera;
    cv::Mat cameraMatrix;
    cv::Mat distCoeffs;
    isfish = camera_id[0] == 'F';
    std::string strId;
    for (size_t i = 1; i < camera_id.size(); i++)
    {
        strId += camera_id[i];
    }
    int id = std::stoi(strId);
    fs["Camera"][camera_id]["pos"] >> pos;
    fs["Camera"][camera_id]["xyzrpy"] >> base2camera;
    fs["Camera"][camera_id]["cameraMatrix"] >> cameraMatrix;
    fs["Camera"][camera_id]["distCoeffs"] >> distCoeffs;
    std::cout << "相机id:" << camera_id << "  isfish:" << isfish << "\nbase2camera\n"
              << base2camera
              << "\ncameraMatrix\n"
              << cameraMatrix
              << "\ndistCoeffs\n"
              << distCoeffs << std::endl;
    assert((isfish && distCoeffs.rows == 4) || (!isfish && distCoeffs.rows == 5)); //鱼眼畸变参数4个，针孔畸变参数5个

    Eigen::Affine3f transform_A; // base_link to 相机外参矩阵
    transform_A = Eigen::Affine3f::Identity();
    transform_A.translation() << base2camera.at<double>(0, 0), base2camera.at<double>(1, 0), base2camera.at<double>(2, 0);
    transform_A.rotate(Eigen::AngleAxisf(base2camera.at<double>(5, 0), Eigen::Vector3f::UnitZ()));
    transform_A.rotate(Eigen::AngleAxisf(base2camera.at<double>(4, 0), Eigen::Vector3f::UnitY()));
    transform_A.rotate(Eigen::AngleAxisf(base2camera.at<double>(3, 0), Eigen::Vector3f::UnitX()));

    p2p_ptr->init(transform_A, cameraMatrix, distCoeffs, pos, id);

    std::thread aa(loop);

    ros::spin();

    if (aa.joinable())
    {
        aa.join();
    }
    else
    {
        cerr << "线程join失败" << endl;
        exit(0);
    }
    flag = false;

    return 0;
}

// int i = 0;
// void ImageCallback(const sensor_msgs::CompressedImage::ConstPtr &img)
// {
//     printf("img get ...\n");
//     cv_bridge::CvImagePtr cv_ptr;
//     try
//     {
//         cv_ptr = cv_bridge::toCvCopy(boost::make_shared<sensor_msgs::CompressedImage>(*img), "bgr8");
//     }
//     catch (cv_bridge::Exception &e)
//     {
//         return;
//     }
//     cv::Mat raw_image = cv_ptr->image;
//     cv::resize(raw_image, raw_image, cv::Size{1920, 1080});
//     // cv::imwrite("/home/zwh/work_space/18xx/calibration-tool/TEST_DATA/hw2camera/mid_car_fish2lidar/" + std::to_string(i) + ".jpg", raw_image);
//     // i++;
// }

// int main(int argc, char **argv)
// {
//     ros::init(argc, argv, "cameraCalib");
//     ros::NodeHandle nh("~");
//     ros::Subscriber sub_img = nh.subscribe<sensor_msgs::Image>("/camera79", 1, &ImageCallback);
//     ros::Subscriber sub_cimg = nh.subscribe<sensor_msgs::CompressedImage>("/camera31_bgr/compressed", 1, &ImageCallback);

//     ros::spin();
//     return 0;
// }